#!/usr/bin/python                             
#-*- coding:utf-8 -*- 
import rospy
import sys
import time 
import socket
import json
from threading import Thread
from robot_pkg.msg import Person1


class Tcp_client(object):

	def __init__(self):
		self.angle_x=0
		self.angle_y=0
		self.angle_z=0
		self.uwb_x=0
		self.uwb_y=0
		self.longitude =0#经度
		self.latitude = 0#纬度
		self.Voltage = 0	 #电压
		self.Current = 0	 #电流
		self.Coulomb = 0	 #剩余容量
		self.Battery = 0	 #容量百分比/电量
		self.Temperature1 = 0	 #温度1
		self.Temperature2 = 0	 #温度2
		self.Temperature3 = 0	 #温度3
		self.data_from_server = "" #服务端数据
		self.Relays1 = 0 #继电器1
		self.Relays2 = 0 #继电器2
		self.Relays3 = 0 #继电器3
		self.Rocker = 0 #摇杆方向
		self.Throttle = 50 #左右电机油门
		self.Gears = 0	#吸污电机的档位
		self.StepperDirection = 0 #步进电机方向
		self.camera_ip1 = ""
		self.camera_ip2 = ""
		self.Water = 0
		print("等待连接服务器......")
		self.socket_client = socket.socket()
		while(True):
			try:
				res=self.socket_client.connect(("192.168.1.10",8899))
				time.sleep(1)
				if not res:
					print("连接服务器成功")
					break
			except:
				pass
		#建立发布者
		self.person_info_pub = rospy.Publisher('/client_publisher', Person1, queue_size=100)

	
	def get_camera_ip(self,msg):
		self.camera_ip1  = msg.camera_ip1
		self.camera_ip2  = msg.camera_ip2
		
	def get_water(self,msg):
		self.Water  = msg.Water
		
	def get_tcp232(self,msg):
		self.angle_x = msg.angle_x/32768*180
		if self.angle_x>180:
			self.angle_x=360 - self.angle_x 
		if msg.angle_x>32768:
			self.angle_x = -self.angle_x
		self.angle_y = msg.angle_y/32768*180
		if self.angle_y>180:
			self.angle_y=360 - self.angle_y
		if msg.angle_y>32768:
			self.angle_y = -self.angle_y
		self.angle_z = msg.angle_z/32768*180
		if self.angle_z>180:
			self.angle_z=360 - self.angle_z
		if msg.angle_z>32768:
			self.angle_z = -self.angle_z

		#self.longitude = struct.unpack('!f',struct.pack('!I',msg.longitude))[0]	
		#self.latitude  = struct.unpack('!f',struct.pack('!I',msg.latitude))[0]
		
		self.Voltage = format(msg.Voltage * 0.01,'.2f')	 	#电压
		self.Current = format(msg.Current * 0.01,'.2f')	 	#电流
		self.Coulomb = msg.Coulomb	 				#剩余容量
		self.Battery = msg.Battery	 	#容量百分比
		self.Temperature1 = format(msg.Temperature1 * 0.1,'.1f')	#温度1
		self.Temperature2 = format(msg.Temperature2 * 0.1,'.1f')	#温度2
		self.Temperature3 = format(msg.Temperature3 * 0.1,'.1f')	#温度3
	#错误处理	
	def Dispose_error(self):
		#关闭客户端
		self.socket_client.close()
		print("关闭客户端")
		#关闭继电器
		person_msg = Person1()
		person_msg.error = 1 #继电器1
		self.person_info_pub.publish(person_msg)
			
	def client_send(self):	
		
		while True:
			#新建字典
			msg= {}
			#添加键值
			msg['Water'] = self.Water
			msg['angle_x'] = self.angle_x
			msg['angle_y'] = self.angle_y
			msg['angle_z'] = self.angle_z
			msg['lon'] = self.longitude
			msg['lat'] = self.latitude
			msg['uwb_x'] = self.uwb_x
			msg['uwb_y'] = self.uwb_y
			
			msg['Voltage'] = self.Voltage
			msg['Current'] = self.Current
			msg['Coulomb'] = self.Coulomb
			msg['Battery'] = self.Battery
			msg['Temperature1'] = self.Temperature1
			msg['Temperature2'] = self.Temperature2
			msg['Temperature3'] = self.Temperature3
			msg['camera_ip1'] = self.camera_ip1
			msg['camera_ip2'] = self.camera_ip2
				
			
			data = json.dumps(msg)
			try:
				self.socket_client.send(bytes(data.encode("UTF-8")))
			except socket.error as e:
				print(f"发生错误1: {e}")
				self.Dispose_error()
				return
			time.sleep(0.5)
			
	def data_procsssing(self,data):
		#处理数据
		if(int(data[0:2],16)==0x55):#开头
			if(int(data[2:4],16)==0x00):#
				#继电器	
				if(int(data[4:6],16)==0x10):
					if(int(data[6:8]) == 0x01):
						self.Relays1 = int(data[8:10],16)#继电器1
						print("继电器1:",self.Relays1)
						self.socket_client.send(bytes((data+"aa").encode("UTF-8")))
					if(int(data[6:8]) == 0x02):
						self.Relays2 = int(data[8:10],16)#继电器2
						print("继电器2:",self.Relays2)
						self.socket_client.send(bytes((data+"aa").encode("UTF-8")))
					if(int(data[6:8]) == 0x03):
						self.Relays3 = int(data[8:10],16)#继电器3
						print("继电器3:",self.Relays3)
						self.socket_client.send(bytes((data+"aa").encode("UTF-8")))
				#电机档位
				if(int(data[4:6],16)==0x20):
					if(int(data[6:8]) == 0x01):#左右电机档位
						self.Throttle= int(data[8:10],16)
					if(int(data[6:8]) == 0x02):#吸污电机档位
						self.Gears = int(data[6:8],16)
				#电机方向
				if(int(data[4:6],16)==0x30):
					if(int(data[6:8]) == 0x01):#左右电机方向
						self.Rocker  = int(data[8:10],16)
					if(int(data[6:8]) == 0x01):#步进电机方向
						self.StepperDirection  = int(data[8:10],16)
				
	def strTolist(self,data):
		list_data=data.split()#分割字符串
		list_len=len(list_data)
		for i in range(list_len):
			print("data",list_data[i])
			print("data type",type(list_data[i]))
			self.data_procsssing(list_data[i])


	def client_recv(self):	
		while True:
	    	# 接收服务端的数据
			try:
				self.data_from_server = self.socket_client.recv(1024).decode("UTF-8")
				if(len(self.data_from_server)!=0):
					print("获取服务端数据：",self.data_from_server)
					#self.strTolist(self.data_from_server)

			except socket.error as e:
				print(f"发生错误2: {e}")
				return
			#time.sleep(0.5)
			   			
	def main(self):
		#self.connectServer()
		try:
			rospy.init_node('Tcp_client', anonymous=True)
			rospy.Subscriber("/tcp232_publisher", Person1, self.get_tcp232,queue_size=100)
			rospy.Subscriber("/cameraIP_publisher", Person1, self.get_camera_ip,queue_size=100)
			rospy.Subscriber("/water_publisher", Person1, self.get_water,queue_size=100)
			#建立客户端读和写的线程
			thread1 = Thread(target=self.client_send)
			thread2 = Thread(target=self.client_recv)
			#线程开始
			thread1.start()
			thread2.start()	
			
	    	#设置循环的频率
			rate = rospy.Rate(5)
			while not rospy.is_shutdown():
				person_msg = Person1()
				list_data=self.data_from_server.split()#分割字符串
				list_len=len(list_data)
				for i in range(list_len):
					print("data",list_data[i])
					if len(list_data[i]) == 10:
						self.data_procsssing(list_data[i])
						person_msg.Relays1 = self.Relays1 #继电器1
						person_msg.Relays2 = self.Relays2 #继电器2
						person_msg.Relays3 = self.Relays3 #继电器3
						person_msg.Rocker  = self.Rocker #摇杆方向
						person_msg.Throttle= self.Throttle#左右电机油门
						person_msg.Gears   = self.Gears   #吸污电机档位
						person_msg.StepperDirection = self.StepperDirection#步进电机方向

						self.person_info_pub.publish(person_msg)
				self.data_from_server = "" #清空接收服务端数据
				rate.sleep()
			#等待线程结束	
			thread1.join()
			thread2.join()	
		except socket.error as e:
			print(f"发生错误: {e}")
			self.Dispose_error()
		#finally:
			#self.socket_client.close()
			#print("关闭客户端")
	


 
 
if __name__ == '__main__':
    try:
        p = Tcp_client()
        p.main()
    except rospy.ROSInterruptException:
        pass
